#include "driver/gpio.h"
#include "driver/pcnt.h"
#include "driver.h"
#include "grbl/plugins.h"
#include "grbl/nvs_buffer.h"

/*
注意：
不可用 UART0 TXD0 GPIO1,RXD0 GPIO3，这两个管脚用于串口通信
另外ESP32 Strapping管脚有GPIO0、GPIO2、GPIO5、GPIO12、GPIO15
这些管脚在上电或者深度睡眠恢复后10ms会做特殊用途，上电结束后可以做普通用途
因为管脚资源缺乏，使用了GPIO2和GPIO12，这些管脚是Strapping管脚，
因此存在潜在问题，开机是如果GPIO_2是高电平导致程序无法上传小概率无法识别串口
GPIO12开机如果是高电平导致无法链接串口

要编译代码将handwheel.c和handwheel.h放入plugins目录
修改my_plugin.c
#include "plugins/handwheel.h"
__attribute__((weak)) void my_plugin_init (void)
{
  handwheel_init();
}
修改根目录线的CMakeList.txt,在set(MISC_PLUGINS_SOURCE项加入
 plugins/handwheel.c
*/

//编码器引脚
#define ENC_A_GPIO  GPIO_NUM_2
#define ENC_B_GPIO  GPIO_NUM_15

//轴选择编码 (GPIO资源有限，这里使用两个管脚来编码x,y,z,off)
//当旋到z时，将接通x,y针脚，为防止干扰可以接入两个二极管隔离
//0 = Off, 1 = x,2 = y,3 = z
#define AXIS_PIN1 GPIO_NUM_39
#define AXIS_PIN2 GPIO_NUM_36

//scale选择
//x1 不接默认就是x1
//SCALE0没有配置，对应程序默认值x1
//SCALE1_GPIO 高x10
//SCALE2_GPIO 高x100
#define SCALE1_GPIO GPIO_NUM_12
#define SCALE2_GPIO GPIO_NUM_14

//没有使用grbl配置指令，要想修改配置更改下面的项
static const int32_t scale_factors[4] = {10, 100, 1000, 1000}; //手轮移动距离缩放，这个配置对应x1,x10,x100,x1000
static const uint32_t jog_velocity = 6000; //编码器控制移动速度单位mm/min
static const int8_t  axis_direction = 1; //控制编码器旋转导致位移方向可以是1,或者-1
static const uint32_t cpr = 200; //每圈脉冲数乘二,程序使用的信号边缘检查，上升或下降都会被检查
//下面为程序变量
static on_execute_realtime_ptr on_execute_realtime = NULL;
static uint8_t axis_encode_last = 0;
static int8_t  axis_i = -1;
static int32_t position = 0;

static bool jog_relative_cmd(int8_t axis,int32_t pos)
{
    static char gcode[50];
    if(axis>=0&&pos!=0){
        sprintf(gcode, "$J=G91 %s%.3fF%u", axis_letter[axis_i], (float)pos/cpr,jog_velocity);
        return grbl.enqueue_gcode(gcode);
    }
    return false;
}

// 正交编码器PCNT初始化
void init_quadrature_encoder() {
    pcnt_config_t pcnt_config = {
        .pulse_gpio_num = ENC_A_GPIO,
        .ctrl_gpio_num = ENC_B_GPIO,
        .lctrl_mode = PCNT_MODE_KEEP,
        .hctrl_mode = PCNT_MODE_REVERSE,
        .pos_mode = PCNT_COUNT_INC,//PCNT_COUNT_DIS,//如果使用PCNT_COUNT_DIS将关闭上升检查
        .neg_mode = PCNT_COUNT_DEC,
        .counter_h_lim = 32767,
        .counter_l_lim = -32767,
        .unit = PCNT_UNIT_0,
        .channel = PCNT_CHANNEL_0
    };
    pcnt_unit_config(&pcnt_config);
    //pcnt_set_filter_value(PCNT_UNIT_0,1023); //滤波器，如果线路存在杂波可以尝试打开
    //pcnt_filter_enable(PCNT_UNIT_0);
    pcnt_counter_pause(PCNT_UNIT_0);
    pcnt_counter_clear(PCNT_UNIT_0);
    pcnt_counter_resume(PCNT_UNIT_0);
}

// 获取当前计数
int16_t get_encoder_count() {
    int16_t count;
    pcnt_get_counter_value(PCNT_UNIT_0, &count);
    pcnt_counter_clear(PCNT_UNIT_0);
    return count;
}

/**
 * grbl.on_execute_realtime执行链被会被grbl主循环调用
 */
static void encoder_execute_realtime (sys_state_t state){
    static uint32_t elapsed = 0;

    uint32_t ms = hal.get_elapsed_ticks(); //返回开机到当前时间的毫秒值

    if(ms != elapsed && (state == STATE_IDLE || (state & STATE_JOG))){
        int axis_pin1 = gpio_get_level(AXIS_PIN1);
        int axis_pin2 = gpio_get_level(AXIS_PIN2);
        uint8_t axis_encode = axis_pin1|(axis_pin2<<1);
        if(axis_encode){
            if(axis_encode_last!=axis_encode){
                //初始化编码位置
                position = 0;
                axis_i = axis_encode-1;
                pcnt_counter_clear(PCNT_UNIT_0);
                if(state&STATE_JOG)
                    grbl.enqueue_realtime_command(CMD_JOG_CANCEL);
            }
            int16_t delta = get_encoder_count();
            if(delta != 0) {     
                // 映射到轴索引 (0=X, 1=Y, 2=Z)
                axis_i = axis_encode-1;
                // 读取倍率选择 (2位二进制)            
                uint8_t scale_idx = gpio_get_level(SCALE1_GPIO)|(gpio_get_level(SCALE2_GPIO)<<1);
                position += (axis_direction * delta * scale_factors[scale_idx]);
                if(jog_relative_cmd(axis_i,position))
                    position = 0;
            }
        }else if(axis_encode_last){
            if(state&STATE_JOG)
                grbl.enqueue_realtime_command(CMD_JOG_CANCEL);
        }
        axis_encode_last = axis_encode;
    }
    elapsed = ms;
    on_execute_realtime(state);
}

// 初始化函数
void handwheel_init() {
    // 1. 初始化编码器
    init_quadrature_encoder();
    // 2. 配置GPIO
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << AXIS_PIN1) | (1ULL << AXIS_PIN2) | 
                       (1ULL << SCALE1_GPIO) | (1ULL << SCALE2_GPIO),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_ENABLE,
        .intr_type = GPIO_INTR_DISABLE
    };
    gpio_config(&io_conf);
    if(!on_execute_realtime){
        on_execute_realtime = grbl.on_execute_realtime;
        grbl.on_execute_realtime = encoder_execute_realtime;
    }
}